import time
import CMDcontrol as CMD
import cv2
import threading
import numpy as np
import rospy
import math
from functools import wraps
import sys

from image_Tag_converter import ImgConverter
from image_Tag_converter import TagConverter

sys.path.append("/home/lemon/catkin_ws/src/aelos_smart_ros")
from leju import base_action


#定义一些参数
Chest_img = None
ChestOrg = None

marker = None
avg_x = None
avg_y = None
avg_yaw = None

chest_circle_x = None
chest_circle_y = None
Debug = 0

real = 1

# 不同色块的hsv范围
color_range = {
    'green': [( 66, 99 , 91 ),( 100 , 165 , 204 )],
    'orange': [( 16 , 191 , 107 ),(21 ,255 , 196 )]
}

def timefn(fn):
    """计算性能的修饰器"""
    #
    @wraps(fn)
    def measure_time(*args, **kwargs):
        t1 = time.time()
        result = fn(*args, **kwargs)
        t2 = time.time()
        print(f" {fn.__name__} took {t2 - t1: .5f} s")
        return result

    return measure_time



#获取图像
def get_img():
    global Chest_img,ChestOrg
    global ret
    image_reader_chest = ImgConverter()
    while True:
        ret, ChestOrg = image_reader_chest.chest_image()
        time.sleep(0.3)
        if ChestOrg is not None:
            Chest_img = ChestOrg
            time.sleep(0.05)
            #Chest_img = cv2.flip(Chest_img, 1)
        else:
            time.sleep(0.3)
            print("暂时未获取到图像")
th2 = threading.Thread(target=get_img)
th2.setDaemon(True)
th2.start()

#查找方块
def find_box(img,color_name):
    global chest_circle_x, chest_circle_y
    if Chest_img is None:
        print('等待获取图像中...')
        time.sleep(0.3)
    else:
        box_img = img
        box_img_bgr = cv2.cvtColor(box_img, cv2.COLOR_RGB2BGR)  # 将图片转换到BRG空间
        box_img_hsv = cv2.cvtColor(box_img, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        box_img = cv2.GaussianBlur(box_img_hsv, (3, 3), 0)  # 高斯模糊
        box_img_mask = cv2.inRange(box_img, color_range[color_name][0], color_range[color_name][1])  # 二值化
        box_img_closed = cv2.erode(box_img_mask, None, iterations=2)  # 腐蚀
        box_img_opened = cv2.dilate(box_img_mask, np.ones((4, 4), np.uint8), iterations=2)  # 膨胀    先腐蚀后运算等同于开运算
        (contours, hierarchy) = cv2.findContours(box_img_opened, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        if len(contours) != 0:
            area = []
            for cn in contours:
                contour_area = math.fabs(cv2.contourArea(cn))
                area.append(contour_area)
            max_index = np.argmax(area)
            (chest_circle_x,chest_circle_y),chest_radius = cv2.minEnclosingCircle(contours[max_index])
            cv2.circle(img,(int(chest_circle_x),int(chest_circle_y)),int(chest_radius),(0,0,255))
            print('A','x=',chest_circle_x,'y=',chest_circle_y)

            if Debug:
                # cv2.imwrite('image.png',img)
                cv2.imshow("Box", img)
                cv2.waitKey(2000)

        else:
            print('正在寻找目标')

#搬箱子
def goto_box():
    global ID
    if chest_circle_x is None :
        print('等待中获取坐标中...')
        time.sleep(0.3)
    else:
        if chest_circle_x < 275:
            print("正在左侧移 ",chest_circle_x)
            base_action.action("Left3move")
            time.sleep(0.5)
        elif chest_circle_x < 295: 
            print("正在左侧移 ",chest_circle_x)
            base_action.action("Left02move")
            time.sleep(0.5)
        elif chest_circle_x > 365:    
            print("正在右侧移 ",chest_circle_x)
            base_action.action("Right3move")
            time.sleep(0.5)
        elif chest_circle_x > 345:    
            print("正在右侧移 ",chest_circle_x)
            base_action.action("Right02move")
            time.sleep(0.5)
        else:
            if chest_circle_y < 300:  
                print("前进",chest_circle_y)
                base_action.action("FastForward1s")
                time.sleep(0.5)
            elif chest_circle_y >= 340:  
                print("后退",chest_circle_y)
                base_action.action("Back1Run")
                time.sleep(0.5)
            else:
                print("开始抱箱子")
                base_action.action("Forwalk01")
                base_action.action("GrabCube")
                base_action.action("LiftCubeUp")
                ID+=1




if __name__ == '__main__':
    rospy.init_node('image_listener')   #ROS节点初始化
    Tag = TagConverter()
    time.sleep(0.5)
    ID = 0      #选择开始时的位置,从初始位置开始各参数为：ID = 0,step = 1,n = 1
    step = 1
    n = 1      #若调试时从ID=2以后开始,需提前将n改为-1,step改为0
    while ChestOrg is None:
        print('wite')
        time.sleep(0.1)
    if step == 1:  #从初始位置开始,搬箱子并走到tag1或tag2面前
        print('启动')
        base_action.action("FastForward2s")
        while not rospy.is_shutdown():
            if ID == 0:  #搬箱子
                find_box(Chest_img, 'orange')
                goto_box()
                time.sleep(0.1)
            elif ID == 1:
                break